#include "common_include.hpp"
#include "iostream"
#include <boost/thread.hpp>
#include <string>
#include <Config.hpp>

std::string cwd = "./";
bool nextstep = false;

void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, void* nothing)
{
    if (event.getKeySym () == "space" && event.keyDown ())
        nextstep = true;
}

void loadfile(const std::string& filename, CloudType::Ptr cloud_ptr)
{
    int res = pcl::io::loadPLYFile(filename, *cloud_ptr);
    if (res < 0) {
        std::cout << "load file failed->filename: " << filename << std::endl;
        std::cin.get();
    }
}

int main(int argc, char **argv)
{
    
    visionhw::Config::setConfigFile(cwd + "config.json");

    int r_s = 255, g_s = 255, b_s = 0; // 源点云颜色
    int r_t = 255, g_t = 255, b_t = 255; // 目标点云颜色
    int r_ss = 0, g_ss = 255, b_ss = 0; // 源点云变换后颜色

    // 可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);

    // viewer->addCoordinateSystem(1);
    viewer->initCameraParameters();
    //viewer->setBackgroundColor(1.0, 0.5, 1.0);
    viewer->registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);

    int k = 0;
    
    CloudType::Ptr mcloud_s(new CloudType); // 源点云
    CloudType::Ptr mcloud_s_voxel(new CloudType); // 源点云滤波后
    CloudType::Ptr mcloud_s_ransac(new CloudType); // 源点云去平面
    CloudType::Ptr mcloud_s_cluster(new CloudType); // 源点云聚类
    CloudType::Ptr mcloud_t_voxel(new CloudType); // 目标点云滤波后
    CloudType::Ptr mcloud_s_sacia(new CloudType); // 源点云粗配准后
    CloudType::Ptr mcloud_s_icp(new CloudType); // 源点云精细配准后

    loadfile("outputply/原始源点云.ply", mcloud_s);
    loadfile("outputply/下采样滤波后.ply", mcloud_s_voxel);
    loadfile("outputply/去平面后.ply", mcloud_s_ransac);
    loadfile("outputply/聚类后.ply", mcloud_s_cluster);
    loadfile("outputply/标准点云滤波后.ply", mcloud_t_voxel);
    loadfile("outputply/粗配准后.ply", mcloud_s_sacia);
    loadfile("outputply/精细配准后.ply", mcloud_s_icp);

    int fontsize = 18;
    int x_pos = 10;
    int y_pos = 40;
    float tr = 1, tg = 1, tb = 1;
    // 展示原始点云
    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_0(mcloud_s, r_s, g_s, b_s);
    viewer->addPointCloud<PointType>(mcloud_s, color_0, "初始源点云");
    viewer->addText("original cloud", x_pos, y_pos, fontsize, tr, tg, tb, "text");

    while (!viewer->wasStopped())
    {
        viewer->spinOnce();
        
        if (nextstep)
        {
            if (k == 6) 
                --k;
            else
                ++k;

            switch (k)
            {
                case 1:{
                    // 展示下采样滤波后的点云
                    viewer->removePointCloud("初始源点云");
                    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_0(mcloud_s_voxel, r_s, g_s, b_s);
                    viewer->addPointCloud<PointType>(mcloud_s_voxel, color_0, "源点云下采样滤波后");
                    viewer->updateText("original cloud after voxel filter", x_pos, y_pos, fontsize, tr, tg, tb,"text");
                    break;
                }
                case 2:{
                    // 展示去平面后的点云
                    viewer->removePointCloud("源点云下采样滤波后");
                    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_1(mcloud_s_ransac, r_s, g_s, b_s);
                    viewer->addPointCloud<PointType>(mcloud_s_ransac, color_1, "源点云去平面后");
                    viewer->updateText("original cloud after removeing plane", x_pos, y_pos, fontsize, tr, tg, tb,"text");
                    break;
                }
                case 3:{
                    // 展示聚类后的点云
                    viewer->removePointCloud("源点云去平面后");
                    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_2(mcloud_s_cluster, r_s, g_s, b_s);
                    viewer->addPointCloud<PointType>(mcloud_s_cluster, color_2, "源点云聚类后");
                    viewer->updateText("original cloud after cluster", x_pos, y_pos, fontsize, tr, tg, tb,"text");
                    break;
                }
                case 4:{
                    // 展示聚类后的点云和目标点云的相对位置
                    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_3(mcloud_t_voxel, r_t, g_t, b_t);
                    viewer->addPointCloud<PointType>(mcloud_t_voxel, color_3, "目标点云滤波后");
                    viewer->updateText("original cloud with target cloud", x_pos, y_pos, fontsize, tr, tg, tb,"text");
                    break;
                }
                case 5:{
                    // 展示粗配准结果
                    try{
                        viewer->removePointCloud("icp结果");
                    }
                    catch(...){}
                    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_4(mcloud_s_sacia, r_ss, g_ss, b_ss);
                    viewer->addPointCloud<PointType>(mcloud_s_sacia, color_4, "粗配准结果");
                    viewer->updateText("sac_ia result", x_pos, y_pos, fontsize, tr, tg, tb,"text");
                    break;
                }
                case 6:{
                    // 展示icp结果
                    viewer->removePointCloud("粗配准结果");
                    pcl::visualization::PointCloudColorHandlerCustom<PointType> color_5(mcloud_s_icp, r_ss, g_ss, b_ss);
                    viewer->addPointCloud<PointType>(mcloud_s_icp, color_5, "icp结果");
                    viewer->updateText("icp result", x_pos, y_pos, fontsize, tr, tg, tb,"text");
                    break;
                }
                default:{}
            }

            nextstep = false;
        }
    }

    return 0;
}